#include "Pid.h"

Pid_t *p1;
Pid_t *p2;

Pid_t* Pid_InitStruct(float p, float i, float d, float range_i, float range)
{
    Pid_t *u = (Pid_t*)malloc(sizeof(Pid_t));
    if(u == NULL) return NULL;
    
    *u = (Pid_t){
        .p = p,
        .i = i,
        .d = d,
        .range_i = range_i,
        .range = range,
        .error = {0, 0, 0},  // 初始化误差数组
        // 其他成员自动初始化为0
    };
    return u;
}

float pid_Process_Incremental(Pid_t* p, float current, float target)
{
    if(p == NULL) return 0.0f;

    // 更新状态
    p->current = current;
    p->target = target;
    
    // 滑动窗口保存误差历史
    p->error[2] = p->error[1];
    p->error[1] = p->error[0];
    p->error[0] = target - current;  // 当前误差

    // 死区处理
//    if(fabs(p->error[0]) <= p->range) {
//        return 0;
//    }

    // 增量式PID计算
    // Δu(k) = Kp*[e(k)-e(k-1)] + Ki*e(k) + Kd*[e(k)-2e(k-1)+e(k-2)]
    float delta = p->p * (p->error[0] - p->error[1]) +
                 p->i * p->error[0] +
                 p->d * (p->error[0] - 2*p->error[1] + p->error[2]);

    // 积分限幅（对增量进行限制）
		if(fabs(delta) > p->range_i) {
				delta = (delta > 0) ? p->range_i : -p->range_i;
		}

    // 更新输出
    p->delta_out = delta;
    // p->out += delta;
    
    return p->delta_out;
}
